from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Port, Direction, Stop, Button, Color
from pybricks.tools import wait

# Initialize the hub.
hub = TechnicHub()
status = 0 # 0: OK, 1 Warning, 2: Error

# Initialize the motors.
steer = Motor(Port.C)
front = Motor(Port.A, Direction.COUNTERCLOCKWISE)
rear = Motor(Port.B, Direction.COUNTERCLOCKWISE)

# Lower the acceleration so the car starts and stops realistically.
front.control.limits(acceleration=2000) #1000
rear.control.limits(acceleration=2000) #1000

# Connect to the remote.
remote = Remote()
#print(remote.address()) Not supported yet
#remote.light.on(Color.CYAN)

# Find the steering endpoint on the left and right.
# The middle is in between.
steer.reset_angle(0)
left_end = steer.run_until_stalled(-300, then=Stop.HOLD) # 200
right_end = steer.run_until_stalled(300, then=Stop.HOLD)

#print("Left: ", left_end)
#print("Right: ", right_end)

# We are now at the right. Reset this angle to be half the difference.
# That puts zero in the middle.
steer.reset_angle((right_end - left_end) / 2)
steer.run_target(speed=300, target_angle=0, wait=False)

# Now we can start driving!
while True:
    # LED indicating particular situation
    pitch, roll = hub.imu.tilt()
    #print(pitch, roll)
    #wait(1000)

    status = 0

    if pitch < -10 or roll < -10 or pitch > 10 or roll > 10:
        status = 1

    if pitch < -20 or roll < -20 or pitch > 20 or roll > 20:
        status = 2
    
    if status == 0:
        hub.light.on(Color.GREEN)
    elif status == 1:
        hub.light.on(Color.ORANGE)
    elif status == 2:
        hub.light.on(Color.RED)

    # Check which buttons are pressed.
    pressed = remote.buttons.pressed()
    #print("pressed: ", pressed)

    # Choose the steer angle based on the left controls.
    steer_angle = 0
    if Button.LEFT_PLUS in pressed:
        steer_angle -= 90 # 75
    if Button.LEFT_MINUS in pressed:
        steer_angle += 90

    # Steer to the selected angle.
    steer.run_target(500, steer_angle, wait=False)

    # Choose the drive speed based on the right controls.
    drive_speed = 0
    if Button.RIGHT_PLUS in pressed:
        drive_speed += 100 # 1000
    if Button.RIGHT_MINUS in pressed:
        drive_speed -= 100 # 1000 

    # HOLD if left is pressed, else apply the selected speed.
    if Button.LEFT in pressed:
        front.hold()
        rear.hold()
    elif Button.RIGHT in pressed:
        front.dc(drive_speed / 2)
        rear.dc(drive_speed / 2)
    else:
        front.dc(drive_speed)
        rear.dc(drive_speed)

    # Wait.
    wait(10)

